package ccgrammar;

import java.awt.geom.Line2D;

import processing.core.PMatrix3D;
import processing.core.PVector;

public class Geom {
	public static PVector normal(PVector v0, PVector v1, PVector v2) {
		PVector v = PVector.sub(v1, v0);
		PVector u = PVector.sub(v2, v0);
		PVector normal = new PVector();
		PVector.cross(v, u, normal);
		normal.normalize();
		return normal;
	}

	public static float distance(float x1, float y1, float z1, float x2,
			float y2, float z2) {
		return (float) Math.sqrt(distanceSqr(x1, y1, z1, x2, y2, z2));
	}

	public static float distance(float[] coords1, float[] coords2) {
		return (float) Math.sqrt(distanceSqr(coords1, coords2));
	}

	public static float distanceSqr(float[] coords1, float[] coords2) {
		float dX = coords1[0] - coords2[0];
		float dY = coords1[1] - coords2[1];
		float dZ = coords1[2] - coords2[2];
		return dX * dX + dY * dY + dZ * dZ;
	}

	public static float distanceSqr(float x1, float y1, float z1, float x2,
			float y2, float z2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		float dZ = z2 - z1;
		return dX * dX + dY * dY + dZ * dZ;
	}

	public static float distanceSqr(float x, float y, float z) {
		return x * x + y * y + z * z;
	}

	public static float distanceSqr(float x1, float y1, float x2, float y2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		return dX * dX + dY * dY;
	}

	public static float distance(float x1, float y1, float x2, float y2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		return (float) Math.sqrt(dX * dX + dY * dY);
	}

	public static PVector segmentSegmentIntersection(double aX, double aY,
			double bX, double bY, double cX, double cY, double dX, double dY) {
		double denominator = ((bX - aX) * (dY - cY)) - ((bY - aY) * (dX - cX));
		if (denominator == 0)
			return null;// parallel
		double numerator = ((aY - cY) * (dX - cX)) - (aX - cX) * (dY - cY);
		double numerator2 = ((aY - cY) * (bX - aX)) - ((aX - cX) * (bY - aY));
		double r = numerator / denominator;
		double s = numerator2 / denominator;
		if (s <= 0 || s >= 1 || r <= 0 || r >= 1)
			return null;// colinear
		double x = aX + r * (bX - aX);
		double y = aY + r * (bY - aY);
		return new PVector((float) x, (float) y);
	}

	public static PVector segmentLineIntersection(float aX, float aY, float bX,
			float bY, float cX, float cY, float dX, float dY) {
		float denominator = ((bX - aX) * (dY - cY)) - ((bY - aY) * (dX - cX));
		if (denominator == 0)
			return null;// parallel
		float numerator = ((aY - cY) * (dX - cX)) - (aX - cX) * (dY - cY);
		float numerator2 = ((aY - cY) * (bX - aX)) - ((aX - cX) * (bY - aY));
		float r = numerator / denominator;
		float s = numerator2 / denominator;
		// if (s <= 0 || s >= 1 || r <= 0 || r >= 1)
		if (r < 0 || r >= 1)
			return null;// colinear
		if (r == 0)
			return new PVector(aX, aY);
		float x = aX + r * (bX - aX);
		float y = aY + r * (bY - aY);
		return new PVector(x, y);
	}

	public static PVector segmentRayIntersection(float aX, float aY, float bX,
			float bY, float cX, float cY, float dX, float dY) {
		float denominator = ((bX - aX) * (dY - cY)) - ((bY - aY) * (dX - cX));
		if (denominator == 0)
			return null;// parallel
		float numerator = ((aY - cY) * (dX - cX)) - (aX - cX) * (dY - cY);
		float numerator2 = ((aY - cY) * (bX - aX)) - ((aX - cX) * (bY - aY));
		float r = numerator / denominator;
		float s = numerator2 / denominator;
		// if (s <= 0 || s >= 1 || r <= 0 || r >= 1)
		if (s < 0)
			return null;
		if (r < 0 || r >= 1)
			return null;// colinear
		if (r == 0)
			return new PVector(aX, aY);

		float x = aX + r * (bX - aX);
		float y = aY + r * (bY - aY);
		return new PVector(x, y);
	}

	public static float distToSeg2DSqr(float x1, float y1, float x2, float y2,
			float pX, float pY) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		float vX, vY = 0;
		float t = ((pX - x1) * dX + (pY - y1) * dY) / (dX * dX + dY * dY);
		if (t <= 0) {
			vX = x1 - pX;
			vY = y1 - pY;
		} else if (t >= 1) {
			vX = x2 - pX;
			vY = y2 - pY;
		} else {
			vX = x1 + t * dX - pX;
			vY = y1 + t * dY - pY;
		}
		return vX * vX + vY * vY;
	}

	public static float distToSeg2D(float x1, float y1, float x2, float y2,
			float pX, float pY) {
		return (float) Math.sqrt(distToSeg2DSqr(x1, y1, x2, y2, pX, pY));
	}

	public static PVector lineLineIntersection(double aX, double aY, double bX,
			double bY, double cX, double cY, double dX, double dY) {
		double denominator = ((bX - aX) * (dY - cY)) - ((bY - aY) * (dX - cX));
		if (denominator == 0)
			return null;// parallel
		double numerator = ((aY - cY) * (dX - cX)) - (aX - cX) * (dY - cY);
		double numerator2 = ((aY - cY) * (bX - aX)) - ((aX - cX) * (bY - aY));
		double r = numerator / denominator;
		double s = numerator2 / denominator;
		double x = aX + r * (bX - aX);
		double y = aY + r * (bY - aY);
		return new PVector((float) x, (float) y);
	}

	public static Line2D.Double getOffsetLine(PVector p1, PVector p2,
			float offset) {
		PVector v = PVector.sub(p2, p1);
		v.normalize();
		v.mult(offset);
		float t = v.x;
		v.x = -v.y;
		v.y = t;
		return new Line2D.Double(p1.x + v.x, p1.y + v.y, p2.x + v.x, p2.y + v.y);
	}

	public static float angleTan(PVector a, PVector b) {
		float dot = a.dot(b);
		if (dot == 0) {
			return 0;
		}
		return (float) Math.atan2(a.cross(b).mag(), a.dot(b));
	}
	public static PVector getBetweenRel(float f,PVector a,PVector b) {
		return new PVector((b.x-a.x)*f,(b.y-a.y)*f,(b.z-a.z)*f);
	}
	public static PVector getBetweenAbs(float f,PVector a,PVector b) {
		float factor=f/a.dist(b);
		return getBetweenRel(factor,a,b);
	}
	public static PMatrix3D getRotateMatrix(PVector a,PVector b) {
		PMatrix3D rot = new PMatrix3D();
		PVector axis = b.cross(a);
		float angle = (float) Math.acos(b.dot(a));
		rot.rotate(-angle, axis.x, axis.y, axis.z);
		return rot;
	}
}
